package com.yuneec.droneservice.command;

import com.MAVLink.yuneec.msg_mav_rc_cmd_ack;
import com.yuneec.droneservice.request.IResponse;

/**
 * Created by gjw on 2017/3/27.
 */

public class GetChannelValues extends BaseCommand {

    public GetChannelValues() {
        super(MAV_RC_COMMAND.MAV_RC_CMD_GET_CHS_VAL);
    }

    @Override
    protected short[] body() {
        return null;
    }

    @Override
    public IResponse<int[]> toResponse(msg_mav_rc_cmd_ack ack) {
        int code = ack.code;
        IResponse<int[]> response = new IResponse(ids(), code);
        if (code == MAV_RC_ERROR_CODE.MAV_RC_ERR_SUCCESS) {
            short[] data = ack.params;
            if (data == null || data.length < 1) {
                response.setParam(new int[0]);
                return response;
            } else {
                try {
                    int[] chs = new int[data[0] & 0xFF];
                    int index = 0;
                    for (int i = 1; i < ack.params.length; ) {
                        chs[index] = uIntArrayToShort(ack.params, i);
                        i += 2;
                        index++;
                        if (index >= chs.length) {
                            break;
                        }
                    }
                    response.setParam(chs);
                } catch (Exception e) {
                    response.setCode(MAV_RC_ERROR_CODE.MAV_RC_ERR_UNKNOW);
                }
            }
        }
        return response;
    }

}
